/*
 * localizer.c - localization algorithms
 */

#include "bpcommon.h"

/** 
  * distpow - power of distance 
  * struct rssi_bind *localbind - a pointer to local bind structure
  * struct rssi_bind *bconbind - a pointer to corresponding beacon bind structure
  * returns: power of distance; 
  */
static double distpow(struct rssi_bind *localbind, struct rssi_bind *bconbind)
{
	/*return pow(((double)(fabs(localbind->mean_db - bconbind->mean_db) * 
				(bconbind->sample_cnt / WIFI_SAMPLE_COUNT))), 2);*/
	
	return pow(((double)(fabs(localbind->mean_db - bconbind->mean_db) * 
				(WIFI_SAMPLE_COUNT / WIFI_SAMPLE_COUNT))), 2);
}

/** 
  * calc_sedist - calculate the distance metric using Signal space-Euclidean DISTance algorithm 
  * struct rssi_fgpt *localfgpt - a pointer to local fingerprints structure
  * struct rssi_fgpt *bconfgpt - a pointer to beacon fingerprints structure
  * returns: the resulting distance; 
  */
static int calc_sedist(struct rssi_fgpt *localfgpt, struct rssi_fgpt *bconfgpt)
{
	struct rssi_bind *localbind = localfgpt->bind_head;
	struct rssi_bind *bconbind = bconfgpt->bind_head;
	struct rssi_bind *defaultbind = rssibind_init();
	int ret;
	double sum, result;
	
	sum = 0;
	defaultbind->mean_db = INVALID_RSSI;
	defaultbind->sample_cnt = WIFI_SAMPLE_COUNT;
	
	while (localbind->sample_cnt){
		/* iterate the whole local fingerprint */

		ret = addrcmp(localbind->hashaddr, bconbind->hashaddr);

		if (ret == 0){
			/* local and beacon hear the same AP */
			/*calculate */
			sum += distpow(localbind, bconbind);
			localbind = localbind->next;
			bconbind = bconbind->next;
		}else if (ret < 0){
			/*beacon has a miss */
			/* replace beacon with default bind and calculate */
			sum += distpow(localbind, defaultbind);
			localbind = localbind->next;
		}else{
			/* local has a miss, it is impossible*/
		}
	}

	result = sqrt(sum);

	free(defaultbind);
	
	return result;
}

/** 
  * rssimap_localizer - calculate the distance metric between local fingerprint and beacon fingerprint recorded in the map, 
  * 					and choose position with the minimal distance as local position
  * @struct rssi_map *rssimap - a pointer to the map structure we read fingerprints from
  * @int fusebcon - fuse beacon only flag
  * @FILE *debug - debug output file descriptor
  * returns: the minimum distance localizer calculated; 
  */
double rssimap_localizer(struct rssi_map *rssimap, int fusebcon, FILE *debug)
{
	struct rssi_fgpt *localfgpt = rssimap->fgpt_head;
	struct rssi_fgpt *bconfgpt = (fusebcon == 1) ? rssimap->fgpt_tail->next : localfgpt->next;

	double dist, mindist;
	
	dist = 0;
	mindist = 1000;
	while(bconfgpt->bind_cnt > 1){
		/* go through all beacons in the map */
		/* distance metric may be calculated according to different algorithm */
		dist = calc_sedist(localfgpt, bconfgpt);
		fprintf(debug, "Dist from local to\t%8s:\t%4.2lf\n", bconfgpt->loc, dist);
		if ((dist != 0)&&(dist <= mindist)){
			/* exclude distance 0. it is itself otherwise too good to be true*/
			mindist = dist;
			strcpy(myposition, bconfgpt->loc);
		}
		bconfgpt = bconfgpt->next;
	}

	return mindist;
}

